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Abstract — In this paper, we extend the recent body of work 
on planning under uncertainty to include the fact that sensors 
may not provide any measurement owing to misdetection. This is 
caused either by adverse environmental conditions that prevent 
the sensors from making measurements or by the fundamental 
limitations of the sensors. Examples include RF-based ranging 
devices that intermittently do not receive the signal from beacons 
because of obstacles; the misdetection of features by a camera 
system in detrimental lighting conditions; a LIDAR sensor that 
is pointed at a glass-based material such as a window, etc. 

The main contribution of this paper is twofold. We first 
show that it is possible to obtain an analytical bound on the 
performance of a state estimator under sensor misdetection 
occurring stochastically over time in the environment. We then 
show how this bound can be used in a sample-based path 
planning algorithm to produce a path that trades off accuracy 
and robustness. Computational results demonstrate the benefit of 
the approach and comparisons are made with the state of the art 
in path planning under state uncertainty. 

Keywords — Path planning, Belief space planning, Autonomous 
systems, Localization. 



I. Introduction 

Map-based, GPS-denied navigation often relies on the mea- 
surement of environmental features to perform state estimation. 
Whether these features are extracted from camera or LIDAR 
data, or supplied by range beacons, their measurement will 
likely be corrupted by noise. Producing a consistent estimate 
in the presence of sensor noise is often the primary concern in 
designing a state estimator, but an important related question is 
how to navigate robustly when the sensor does not produce a 
measurement at all, either because the sensor itself is corrupted 
or environmental conditions prevent the sensor from making a 
measurement. Examples of such situations include RF-based 
ranging devices that intermittently do not receive signals from 
beacons due to obstacles, and misdetection of features by a 
camera system in textureless areas of the environment or due 
to adverse lighting conditions. 

The main contribution of this paper is twofold. We first 
show that it is possible to obtain an analytical bound on the 
performance of a state estimator under probabilistic sensor 
misdetections. We then show how this bound can be used in 
a sample-based path planning algorithm that minimizes goal- 
state uncertainty under such a stochastic mode of misdetection. 

Recent work in robotics has emphasized robust path plan- 
ning under various sources of uncertainty. The objective is 
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frequently to identify a feasible start-to-goal path that mini- 
mizes uncertainty along the path, uncertainty at the goal, or 
some combination of these and the length of the path. Actions 
and measurements affected by noise are often considered, 
as well as uncertain maps. The stochastic motion roadmap 
is a foundational work in modeling path planning under 
process noise as a Markov decision process (MDP), which is 
solved optimally using dynamic programming [1]. Using the 
framework of Partially Observable Markov Decision Processes 
(POMDP), Marthi addresses path planning in environments in 
which obstacles appear/disappear dynamically over time [2]. 

If stochastic measurements are considered in addition to 
actions, the planning problem, also a POMDP, is intractable 
over most state spaces relevant to problems in robotics. As a 
result, a variety of algorithms make simplifying assumptions 
and find high-quality feasible paths that manage uncertainty. 
Sample-based motion planning is often utilized to generate a 
set of collision-free feasible paths from which a minimum- 
uncertainty path is selected. The belief roadmap (BRM) [3] 
builds a probabilistic roadmap (PRM) [4] in a robot's state 
space, propagates beliefs over the roadmap using an extended 
Kalman filter (EKF) [5], and plans a path of minimum goal- 
state uncertainty. This approach has been extended [6] to bias 
the PRM samples using a Sensory Uncertainty Field (SUF) [7], 
which expresses the spatial variation in sensor performance 
over the workspace. Rapidly-exploring random belief trees 
(RRBTs) [8] use the EKF to propagate belief states over a 
rapidly-exploring random graph (RRG) [9], to find asymptot- 
ically optimal paths that minimize goal-state uncertainty sub- 
ject to chance constraints. Linear quadratic Gaussian motion 
planning (LQG-MP) [10] pairs an LQG controller-estimator 
duo with trajectories planned using rapidly-exploring random 
trees (RRTs) [11], seeking a path that minimizes the product 
of collision probabilities at all states. Instead of sample-based 
planning, continuous optimization is used by Piatt et al. [12]; 
locally optimal paths are computed directly in belief space 
under the assumption that the maximum-likelihood measure- 
ment is always obtained, and LQG estimation and control are 
applied. 

Some algorithms assume uncertainty in the map used for 
navigation. Missiuro and Roy [13] employ PRM path planning 
over a roadmap in which the positions of obstacle vertices 
are uncertain, and Guibas et al. [14] extend this approach to 
3D workspaces. This methodology is combined with assump- 
tions of uncertain actions and measurements by Kurniawati 
et al. [15], who use a point-based POMDP planner to obtain 
an approximate minimum-cost solution, where the cost is a 
combination of movement and collision risk. A hierarchical 
approach is adopted by Vitus et al. [16], who manage all three 



sources of uncertainty by decomposing the workspace into a 
graph and optimizing over the graph in several steps. Wellman 
et al. [17] consider a setting in which the edge costs on the 
graph are uncertain, with potential probabilistic dependency 
between the costs, and provide a path planning algorithm 
that produces optimal paths under time-dependent uncertainty. 
Acar et al. [18] present an approach that uses geometric and 
topological features instead of sensor uncertainty models. 

Our problem is related to that of planning over a stochastic 
map. Uncertainty in the arrangement of obstacles in a map may 
adversely impact a robot's navigation process, and similarly, 
the precision of a robot's state estimate will be hindered if its 
sensors do not detect a measurement. Our aim is to develop a 
principled method for planning under uncertainty when mis- 
detection by the robot's sensors is a primary concern. In total, 
we consider three sources of uncertainty: process noise, sensor 
noise, and sensor intermittency, for which sensor misdetections 
occur with a known probability. Like the BRM method of 
Prentice and Roy [3], an EKF is used to propagate belief states 
over a PRM, and a path of minimum goal-state uncertainty is 
selected. Performance guarantees are developed that bound the 
filter's performance under probabilistic misdetections by the 
sensors, and the likelihood of these misdetections is considered 
explicitly, along with process and sensor noise, in selecting a 
minimum-uncertainty path. 

Regarding the estimation aspect of the problem, we demon- 
strate that the choice of the expected maximum eigenvalue 
of the error covariance matrix as a metric allows us to 
compute a novel upper bound on its evolution, which is further 
extended to the case of stochastic misdetections by the robot's 
sensors. These bounds are distinct from existing results in 
the literature, surveyed in [19], which are mainly for the 
Algebraic Riccati equation, representing the steady state value 
of the expected error covariance instead of its instantaneous 
value that we are concerned with. Other metrics, such as 
the trace of the expected error covariance matrix, have been 
commonly considered in the past, cf. [3]. However, to the best 
of our knowledge, the trace does not offer a tractable means 
to bound its evolution over time, especially in the stochastic 
setting of sensor misdetections. Further, while introducing 
conservativeness in the sense that we consider the maximum 
mode for the uncertainty, propagating the maximum eigenvalue 
bound offers a computational advantage since we only need to 
propagate a scalar quantity instead of the entire covariance 
matrix. 

This paper is organized as follows. In Section II, we for- 
mulate the problem. An analytical bound on the performance 
of the state estimator with probabilistic sensor misdetections 
is derived in Section III. In Section IV, we describe in detail 
how the analytical bound is used for robust path planning. 
Computational results are reported in Section V. Finally, 
conclusions and future directions are discussed in Section VI. 

II. Problem Formulation 

We consider a general model of an agent whose state evolves 
as per a non-linear discrete-time dynamical system 



where x <E IR™ X is the state describing the system at time 
t, f : W lx x R n " — >• M" x describes the state transition map 
of the system and n £ K." n is the process noise. The agent 
is equipped with m sensors in order to estimate the state x. 
Sensors' output is modeled as 



y J -(t) = h i (x(t),v i (t)) 1 Vje{i, 



*}, (2) 



where Vj 6 



is the process noise of the j-th sensor and h : 
i n "j describes the relation between state and 
measurement. We assume that the noise vectors n and Vj are 
independently generated mean-zero Gaussian random vectors. 

In this paper, we consider situations where sensors can 
misdetect features and thus, do not produce a measurement 
at certain time instants. Examples include RF-based signals 
from beacons that are not detected by the agent because of 
low SNR, misdetection of features using cameras because of 
abrupt change of lighting conditions, no LIDAR returns from 
certain materials, etc. 

Under these circumstances, analogous to the Kalman Filter 
case (see equations 185 and 186 in [20]), an Extended Kalman 
Filter (EKF) based estimator of the state x can be written as: 

m 

P;; 1 ! = (F 4 P t F; + Q,)- 1 + ]T ^.t+iH^^H,, (3) 



x t+1 = P t+ i ((F t P t F' t + Qt)"^ (x t , 0) 



x(t+l)=f(x(t),n(*)), 



(1) 



+ ^ 7j - t+1 H;.RTi +1 (y,(f+l)-h,(f(x f ),0))), 

where x t is the state estimate, P t is the expected error 
covariance with respect to the process and sensor noise terms, 
F t is the linearization of f around (x* , 0) and EL, is the 
linearization of hj around (f(x t ),0). The matrix Q t is the 
process noise covariance and the Rj t+i is the measurement 
noise covariance associated to the j-th sensor. The variables 
7j t+i are binary, 0—1 stochastic variables that model the 
misdetection by the j-th sensor. 

We are interested to compute a BRM that trades off accuracy 
and robustness when sensors can stochastically be in misdetec- 
tion mode during the mission. We will call this Robust Belief 
Roadmap or RBRM. 

In this work, we make the following assumptions: 

Assumption II.l (Misdetection map). For each sensor j, we 
can characterize the misdetection probability (1—pj) at each 
location in the environment. 

Assumption II.2 (Independence). We assume jj, namely 
the misdetections to be independent over time and between 
sensors. Specifically, the jj are Bernoulli random variables 
with pj being the probability of jj = 1. 

For constant values of Pj's, Assumption II.2 is common in 
the literature pertaining to the research area involving estima- 
tion with intermittent observations, e.g., see [21]. However, 
most results in this field are concerned with stability of the 
estimation algorithms, while our focus in this paper is to char- 
acterize the evolution of the estimation performance. Further, 



in our set-up, the parameters pj of the random variables 7^4 
in (3) are functions of the state x, whereas in estimation 
literature, the state dependence is not explicitly considered. 

Assumption II.3 (Consistency). We assume that the state 
estimate x t is identical to the nominal trajectory to be followed 
by the vehicle. 

The last assumption implies that we have a reasonable 
nominal model for the motion of the vehicle, and the sensors 
possess a decent level of accuracy so that the vehicle moves 
close to the nominal trajectory. This work is concerned with 
the level of confidence measured through P t that we can obtain 
in our state estimate x t . 

With these assumptions, we address the following problems: 

Problem ILL Given the sensor accuracies, their misdetection 
probabilities and a nominal trajectory, determine a bound 
on the evolution of the expected value of Ai(P t ), where 
M represents any function that can capture the uncertainty 
through P t . Here, the expectation is taken over the joint 
distribution of the random variables 7^ representing sensor 
misdetection. 

Equipped with this theoretical bound, the second goal is to 
apply the bound to the following problem on path planning 
under uncertainty. 

Problem II.2. Given a set of candidate trajectories from a 
start location to a goal location, develop an algorithm that 
propagates the bound on E[A / ((Pt)] over the PRM, to output 
a path having minimum goal-state uncertainty. 

Remark ILL The probability of misdetection of a sensor 
(Assumption II. 1) is in general difficult to know precisely. 
However, thanks to rather realistic simulation environments 
capable to both simulate sensor responses as well as the 
environment, one can foresee the possibility of obtaining rather 
realistic models. For example, given a geometric model of 
the environment, edges and/or corners of buildings could 
be marked with different misdetection probabilities if some 
information from the type of material, the texture, the time of 
the day the mission is carried out, etc. are considered. Entire 
areas of the environment could be marked with misdetection 
probabilities, e.g., modeling the fact that an RF-signal cannot 
easily be detected behind buildings. This could be obtained 
with ray tracing based algorithms, see, e.g., Figure 1. 

The misdetection probability of some sensor could also be 
determined from historical data collected in the mission, for 
example correlating some information about the environment, 
such as obstacle density, time of the day the mission was 
carried out, etc. with the misdetection state of a sensor. 

Of course, it is impossible to capture all sources of un- 
certainty. However, if some of this information is available, 
the RBRM method can take it into consideration trading off 
accuracy and robustness. 

When such models are only of qualitative nature, the RBRM 
could be used by the user to assess the robustness of the 
solution. In practice, the misdetection probabilities can be 
used as user parameters. Varying such parameters, the user 
can study how the RBRM changes under, for example, more 
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Fig. 1. Illustration of the model of the environment which could be used 
for planning. In this case, not only the geometric information about the 
obstacles and position of beacons is considered, but also the information about 
the accuracy certain sensors will have in detecting, e.g. edges of buildings 
and signal from a radio tower (beacon). Note that in this scenario a rather 
elaborate model is used, where edges are categorized into three classes: 
detectable, detectable with 85% and 60% probability. This different detection 
accuracy could be caused by the sun's position. Signal strength map from 
radio tower could be determined using ray tracing algorithms. In this example, 
just for illustration purposes, we have three regions with different detection 
probabilities. 



pessimistic hypothesis on the behavior of certain sensors, 
having higher misdetection probability in more remote regions 
of the environment where more uncertainty is expected, etc. 

III. Analytical Bound on Performance 
In the seminal work by Prentice and Roy [3], it was shown 



that if the covariance matrix is factorized as Pt 



B f c: 



then the time evolution of the terms B t , C t is linear. This 
enabled the authors to develop and demonstrate a computation- 
ally efficient algorithm to compute a roadmap that captures the 
estimation accuracy. The function A4 used in [3] is the trace 
of the matrix. Leveraging the same factorization, one arrives 
at the following equation: 



B, 

C, 



M( 7t )F t 



QtvFr 1 )' 



(F-iy + M^q^F- 1 ) 



B t _ 

c*. 



(4) 



where M(7t) = Y^T=i 7j,*H'-R~ t 1 Hj, which depends on the 
stochastic variables jj t . This can be thought as a transfer 
function that maps the matrices B t and C t from one node 
of the roadmap to the next [3]. In this stochastic setting, 
however, a direct application of the BRM proposed by Prentice 
and Roy in [3] requires extensive Monte Carlo simulations 
over all the variables 7^, Even if the factorization provides 
a faster computation of the covariance, the method becomes 



very quickly intractable, especially if 7^ changes spatially. 
See, e.g., the signal received from a radio tower as in Figure 1. 

A way to mitigate this is by taking the expectation with 
respect to the 7^, namely by computing E(B f ) and E(C t ). 
This would enable us to compute an expected transfer func- 
tion between two nodes in the roadmap. However, note 
that E(P t ) = E(B t C ( T 1 ) ^ E(B t )(E(C t ))" 1 , thus preventing 
us to compute what the expected state covariance is at each 
node of the roadmap. 

In order to both obtain a meaningful metric on the ex- 
pected error covariance, which captures the estimate accuracy 
when there are sensor misdetections, and have computational 
tractability, we establish a bound on the largest eigenvalue of 
the covariance. Intuitively, we are approximating the uncer- 
tainty at each node of the roadmap with a ball whose radius 
is the largest eigenvalue of the covariance and we determine 
a bound on such radius. 



A. Bound with all sensors functioning 

In order to derive a bound on the maximum eigenvalue of 
the expected error covariance, we consider first the case when 
all the sensors are in working condition at all times. Such 
analysis is instrumental to derive a bound for the case of sensor 
misdetection. 

Without loss of generality and for the sake of simpler 
notation, we assume that there exists only one sensor that is 
in working condition. The bound can be easily generalized to 
multiple sensors. In the following, we will denote with A(A) 
and with A(A) the minimum and maximum eigenvalue of A, 
respectively, where A is a positive definite matrix. 

For the expected error covariance, the following result 
provides a recursion to compute an upper bound on A(P t ). 

Theorem III.l. At every time instant t, 



Theorem III.2. Suppose that the cardinality of Xg is k. Then, 
under Assumption II. 3, at the final estimation time instant T, 



A(P t ) < 



A 2 (F t )A(P t _ 1 ) + A(Q t ) 

A (HJR,- 1 !^) (A 2 (F t )A(P t _x) + A(Q t )) + l' 



(5) 



where the Jacobians F t ,H t are evaluated at f(x t _i,0). 

The proof of Theorem III. 1 is reported in the Section A of 
the Appendix. This result is stated in the form of a recursion 
mainly because the terms A 2 (F t ) and HjR(" 1 H t are functions 
of the estimate x f _i. If we can uniformly upper and lower 
bound them respectively, e.g, in the linear time invariant case, 
then we can state a uniform upper bound for A(Pt) as a 
function of the initial value Po, given by the next result. 

For this result, we introduce the following notation. Let 
X :— {x , x 1; . . . , x^}, denote a set of estimates at different 
times. Under Assumption II. 3, this set is identical to the 
nominal trajectory sampled at the corresponding times. Define, 

Xs ■= {x € X : a(h(i(x, 0))'R^ 1 H(f (x, 0))) = o}, 

denote the subset of X at which the sensor provides no useful 
information. The filter runs open-loop at these locations. 
Then, the following result holds. 



A(P T ) <bJ2 a ^ 1 - aK C + a ' i 
3=1 

1 ( 




_ jd-Qc) 1 - 

(Cc+a) T ~ 

C + A(P ) ' (c + a \ 1-&M 

x (Cc+a) 



a := sup A(F(x)) , 6:=supA(Q t ), 

ie* t 

c:= inf A (H^MjyB^H^f (*,()))) , 

d:=bc/a + l, C'-={d-a + yj{d - a) 2 + Abe) /{2c) . 

The proof of this result is presented in Section A of 
the Appendix. To extend both of these results for m func- 
tioning sensors, we simply replace the term HJR^Ht by 

E m II' B _1 H 
i=i H jAf H i.'' 

B. Bound under Stochastic Sensor Misdetections 

In this section, we extend the analysis to the case of multiple 
sensors, added to improve robustness, but that can produce 
misdetections as per Assumptions II. 1 and II. 2. The metric 
which we analyze is E[A(Pt)], where the expectation is taken 
over the stochastic process of sensor misdetections. 

For brevity, let £ t := A(P t ). For k € {1, . . . , rn}, define: 



wi,...,i fc 



= A 



(2 H <i,t R «i!t H *i.*)' 



j=i 



di u ...,i k =bc iu ... tik /a + l, 

where a tuple ii, . . . , ik is a subset of {1, ... , m}, and a, b are 
as defined in Theorem III. 2. 

Using this notation, we have the following recursion which 
provides an upper bound on E[^ t ]. 

Theorem III.3 (Stochastic misdetections). Under Assump- 
tions II. 1 and II. 2, at any given time instant t, 

E[£ t ] < (dE&_i] + b) ((1 - Pl ) ... (1 - Pm ) 

, Pl(l -P 2 ).-.(1 ~Pm) , , (1-Pl)(l-P2)---Pm 



ciE[4-i] + di 

PlP2 ■■■ (1 - Pm) 

ci2E[4_i] + d u 



CmE[4_i] +d m 
(1 -pi). ..Pm-lPm 
Cm— l,rn^- J [^-t— lj ' (*m— l,m 



pi...p„ 



ci,..., m E[^_i] + di,..., 

The proof of this result is presented in Section A of the 
Appendix. 

This bound requires the enumeration of all of the 2 m 
possibilities of sensor combinations, and therefore, the com- 
putational complexity scales undesirably with m. One way to 



derive an efficient bound is to obtain a uniform lower bound 
c on each of the c's. In that case, the common denominator of 
the right hand side terms becomes cE[£ t _i] +d. The recursion 
then simplifies to 



E[£ t ]<(aE[£ t ^+b) ( [](1 



■Pi, 



i-n; n = i(i-Pi ) 

cE\£ t -i]+d 



This recursion can be evaluated on similar lines to the proof of 
Theorem III. 2 to obtain an upper bound on E[£ t ] as a function 
of E[£ ]. 

For certain types of sensor suites, one may be able to derive 
a slightly conservative, but computationally efficient upper 
bound which we report next. 

Corollary III.l (Simplified bound). Under Assumptions II. 1 
and II. 2, at any given time instant t, 



E[£ t+1 ] < (aE[£ t ]+b) 




■Pj) 



P.i 



^ c i E &l 



The proof is reported in the Section A of the Appendix. The 
main advantage of this bound is the computational efficiency 
as compared to the one in Theorem III. 3. However, this bound 
requires at least one of the sensors to have its c value to be 
strictly positive, and therefore, may become too conservative. 

IV. Application to Path Planning Missions 

The upper bound on E[£ t ] given in Theorem III. 3, which 
we refer to hereafter as E[£ t ], may be used to plan paths of 
minimum expected goal-state uncertainty in a manner similar 
to the belief roadmap algorithm [3]. We will assume that a 
probabilistic roadmap with node set N and edge set E is 
provided as input, along with beliefs /j,q and [i goa i defining 
a start state and goal state on the roadmap. We also assume 
that for every node n £ N, the triple n — {/i,E[£],7r} is 
stored, which contains the belief, the eigenvalue bound, and 
the path 7r (beginning at /i ) associated with this node. We 
refer to individual members of the triple using the notation 
n[ij], n\E[£]}, and n\it\. Belief propagation and graph search 
proceeds similarly to that of the standard BRM algorithm; E[£ t ] 
is propagated according to the recursive inequality given in 
Theorem III. 3, and is used in place of the nominal-trajectory 
expected error covariance matrix that is propagated in the 
standard BRM. We assume the bound is used to compute a 
transfer function E[£], = £(i, Z,E[£].), that takes as input the 
indices of an edge e^z in the roadmap, and the eigenvalue 
bound associated with node m. In the context of the graph 
search, we treat E[£] independently of time, and assume that 
rij[E[^]] represents the best-yet covariance eigenvalue bound 
identified at n,. The search process is shown in Algorithm 1. 

The use of our proposed approach, i.e., propagation of E[£t], 
provides us with significant computational advantage over ex- 
isting methods such as [3]. If we were to use their factorization 
from (4), then we would have to compute: 1) 2 m realizations of 
the matrix B t (one for each subset of misdetecting sensors), 2) 



Algorithm 1 n goa i [tt] = RBRM(/j, ,ij, goal ,E[£] , N,E) 
for e u e E do 

£(i,l,'E[£} i ) <— PropagatcBound(ei i () 
end for 

Q <- n Q = { M o,E[£] o ,0} 
while Q ^ do 
m 4- Pop(Q) 
for m £ en do 
if ni ^ m [tt] then 
EpJ^ £(» ,l 1 nS\l]}) 
if £[£}, <ni[E[£}] then 

ni «- {nj[/j],E[/]j,nj[7r] U n t } 
Q^Push(Q,m) 
end if 
end if 
end for 
end while 
return n goal [Tr] 



inverses of 2 m realizations of the matrix C t , 3) multiply each 
realization of B t with corresponding C^ 1 and finally, 4) sum 
up the 2 m terms to compute E[P t ] or its trace. Instead, our 
approach requires the computation of minimum eigenvalues 
of 2™ much smaller sized matrices, i.e., sums of the terms 
H'.R~j Hj, for which efficient algorithms exist even for larger 
sizes [22], along with step 4) of the above, which provides 
significant savings in high dimensional state space. 

V. Computational Results 

We next plan paths of minimum uncertainty under process 
noise, sensor noise, and probabilistic misdetections for a planar 
Dubins vehicle [23] in an environment populated with obsta- 
cles. We assume a robot is using three sensors for navigation: 
ultra-wideband (UWB) range beacons, a laser rangefinder for 
measuring obstacle vertices, and odometry that is subject to 
drift over time. The beacons provide measurements throughout 
the workspace, but their noise properties are assumed to vary 
as a function of distance to the robot 1 , according to 



v J -(t)~-V(0, o -(d^(t)) 2 ) ; 

<r(dj(t)) — adj(t) + (To • 



(6) 

(7) 



The noise associated with the range measurement of beacon j 
has a standard deviation that varies linearly in the Euclidean 
distance dj(t) between the robot and sensor j. The standard 
deviation takes on value er at range zero and increases 
according to the coefficient a. For the laser rangefinder, we 
assume the measurement of range to an obstacle vertex is 
corrupted by Gaussian white noise with properties that do not 
vary spatially, and the vertices measured are always correctly 
associated with a prior map. The maximum range of the laser 
is limited, however, and obstacles can only be detected in close 
proximity to the robot. 

1 A more general model could also consider a bias term as described in [3] . 
This could be easily added also in our framework. 
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Fig. 2. Planned paths in a workspace populated with obstacles (measured by 
laser) and UWB beacons. The robot receives the beacon measurements with 
probability 0.1, and extracts obstacle corners from laser data with probability 
0.9. At top, a path planned using It as a performance metric, neglecting all 
probabilistic sensor misdetections. At bottom, a path planned using ¥,[£t] as 
a performance metric, which considers the misdetection probability of each 
sensor. The UWB beacons are queried at every measurement iteration; the 
laser has a range of one unit and its planned measurements are rendered (green 
for a successful measurement and red for a misdetection) for a representative 
failure scenario. Ninety-five percent confidence covariance ellipses are plotted 
at regular intervals along each path. 



A start state and goal state are designated for the robot, 
and a PRM is used to identify feasible paths between the start 
and goal. To select the path of minimum goal-state uncertainty, 
two methodologies are compared: the original BRM algorithm, 
with no notion of sensor misdetections, and the proposed 
RBRM algorithm, which uses E[£ t ] as a cost metric instead 
of tr(Pt). For all path planning scenarios investigated, the 
standard BRM algorithm was found to choose the same path 
regardless of whether tr(P t ) is used as the cost metric or 
£ t is used instead. Evaluating £ t over the roadmap offers a 
better comparision with E[£ t ], and so both tr(P t ) and £ t are 
computed for comparison with E[£ t ] in the results to follow. 

The first scenario considered is illustrated in Figure 2, in 
which a robot must plan from start to goal in a workspace 
populated with three obstacles and four range beacons. Very 
simple collision-free paths are evident through the upper 
reaches of the workspace, but in cases where the probability 
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Fig. 3. At top, the propagation in time of the eigenvalue performance metrics 
over the paths in Figure 2. At bottom, ir(Pt) is also given for both paths. 
All quantities except E,[lt] represent the mean over one hundred Monte Carlo 
trials in which different sequences of sensor misdetections occur according to 
the prescribed probabilities. 



of UWB misdetection is high, it is advantageous for the robot 
to travel through the obstacles to collect laser measurements 
that reduce position uncertainty. For the specific case plotted in 
Figure 2, the beacons have a ten percent probability of deliver- 
ing a successful measurement to the robot, and the laser (with 
a maximum range of one unit) has a ninety percent probability 
of successfully extracting an obstacle vertex and measuring its 
range to the robot. When intermittent sensing is neglected, the 
robot takes a short path to the goal that collects measurements 
from the UWB beacons only. When intermittent sensing is 
considered, the robot takes a detour through the obstacles to 
reduce the uncertainty of its state estimate. For both paths, one 
hundred Monte Carlo simulations were performed in which 
sensors fail according to the prescribed probabilities, and the 
resulting mean values of tr(P t ) and £ t are compared with 
EptJ in Figure 3. 

This planning scenario is next considered over a range of 
different misdetection probabilities, for both the laser and the 
UWB beacons, and the results are summarized in Figure 4. 
The number of planned laser measurements in the minimum 
uncertainty path, computed using E[£ t ], is given at top, and 
the value of E[£ t ] at each path's goal state is given at bottom. 
The zero-range noise level <7q selected for the UWB beacons 
is an order of magnitude lower than the constant variance 
representing the laser noise, and so the UWB beacons are used 
exclusively for all scenarios in which they are more than fifty 
percent reliable, even if the laser is more reliable. 
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Fig. 4. Characteristics of a planned path are plotted as a function of 
sensor reliability, for the two-sensor example shown in Figure 2. At top, the 
covariance eigenvalue bound at the goal state is illustrated, and at bottom, the 
number of planned laser measurements along the selected path is illustrated. 
The upper left corner of each plot corresponds to the parametrization used in 
Figures 2 and 3. 



A final path planning test case with continuously varying 
sensor intermittency is considered in Figure 5. In a workspace 
populated with eight obstacles and no UWB beacons, we 
assume that a light source causes the expected sensing inter- 
mittency to vary continuously along the vertical axis of the 
workspace, with a high detection probability at bottom and 
a low detection probability at top. Neglecting sensor inter- 
mittency, the standard BRM algorithm plans a path through 
the upper region of the workspace, and considering sensor 
intermittency, planning with K[£ t ] yields a path that collects 
many high-probability measurements from the lower region of 
the workspace to minimize uncertainty at the goal state. The 
candidate metrics, averaged over one hundred simulated cases 
of sensor intermittency, are given in Figure 6. 

VI. Conclusion and Future Directions 

In this paper we have described how to plan when sensors 
used for state estimation are not only noisy but may fail to 
produce measurements because of misdetections. Being able 
to tradeoff both accuracy and robustness is very appealing as 
autonomous vehicles heavily rely on complex sensors such as 
cameras and LIDAR whose capability of extracting relevant in- 
formation, such as features and point clouds, strongly depends 
on environmental information that can be predicted to a certain 
extent, such effect of lighting conditions, type of surfaces, etc. 
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Fig. 5. Planned paths in a workspace over which the probability of a 
successful corner detection varies spatially along the vertical axis. Obstacles 
measured at bottom have the highest probability of a successful measurement, 
and obstacles measured at top have a near-zero probability of a successful 
measurement. At top, a path planned using It as a performance metric, 
neglecting all probabilistic sensor misdetections. At bottom, a path planned 
using ¥,[£t] as a performance metric, which considers the misdetection 
probability of each sensor. The laser has a range of one unit and its planned 
measurements are rendered (green for a successful measurement and red for 
a misdetection) for a representative scenario. Ninety-five percent confidence 
covariance ellipses are plotted at regular intervals along each path. 



Even in the case when such information is not fully available, 
the proposed methodology can be very beneficial to study the 
robustness of the path to intermittent sensing, by testing the 
robust roadmap, for example, by choosing various probabilities 
of intermittency at various location in the map. 

Future directions include the incorporation of map uncer- 
tainty within the current framework and the use of the current 
analysis for multi-objective path planning. 
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Appendix 

In this appendix, we have included the proofs of all technical 
results presented in this paper. 

To prove Theorem III. 1 , we will use the following set 
of matrix identities. Although these inequalities are fairly 
standard, we provide a proof for the sake of completeness. 

Lemma A.l ([24]). Given n x n positive definite matrices 
X, Y.Z, 



X(X + Y)> X(X) + X(Y) 

X(X + Y)<X(X) + X(Y) 

X(XY) < X(X)X(Y) 



(8) 

(9) 

(10) 



where A, A denote the minimum and tlie maximum eigenvalues 
of the matrix, respectively. 

Proof: We prove only the max. The min part follows 
analogously by reversing signs. To prove (9), recall that 

t/ v ,« 11(^ + ^)^11 ^ \\Xx\\ \\Yx\\ 

X(X + Y) = max — — „ „ ' " < max "„ ,," + max ^— - 

xjtO \\x\\ x^O \\x\\ x^O \\x\\ 

= X(X) + X(Y), 

where we used the triangle inequality in the second step. (10) 
follows by using the same steps, except that we use the sub- 
multiplicativity of the norm to obtain the inequality. ■ 

We are now ready to establish Theorem III. 1. 

Proof of Theorem III.l: For the recursion, we use the 
following inverse covariance form [20] 

Pr 1 = (Ft-iPt-iFj.! + Qt-i)- 1 + HjR^H,, 

where F, H are Jacobians of f , h around the state prediction 
at time t — 1. Using the bounds in Lemma A.l, we obtain 

A(P t ) = A( [hJR^H, + (Ft-iPt-iFi.! + Qt-i)~ 
1 



(8) 
< 



AJHJR^IL + (F^Pt-iFi.! + Q t _i)- 

1 
A (H'tR^Ht) + \((F t - 1 P t - 1 F' t _ 1 + Qt-i)- 1 ) 

1 

AtHJR^Ht) + (MFt-iF-t-iF'^+Qt-i))- 1 



(9),(10) 
< 



A 2 (F t _ 1 )A(P t _ 1 ) + A(Q t _ 1 ) 

A (H' t R t - x H t ) (A 2 (F t _ x )A(P t _i) + A(Q t _i)) + l' 



To prove Theorem III. 2, we require the following interme- 
diate result, which establishes a bound for the value of a scalar 
variable I which may evolve as per one out of two equations 
at any given time. 

Lemma A.2. Suppose that in the time interval [0, 1, . . . ,T], 
a scalar variable £ evolves as per 



ai t + b 



for some T — k instants, 



£ t+ i = {ci t + d' 

al t + b, for the remaining k instants, 



where a, b, c, d are some finite positive scalars, then 

iT^b^aJ- 1 -(a K + a K / 

- , x t-k -, /i _ (rf-Cc) T ~" 

d-Cc \ 1 C I (Cc+q) T -' 

£c+a C + lo Cc + a\ i _ MzM 

\ (Cc+a) 

where ( is defined in Theorem III.l. 

Proof: Observing that for the above evolution of I, 

al + b , a(al + b) + b 
• b > 



cl + d 



cl + d 



which means that for any sequence of k occurrences of the 
second equation, one can always upper bound the resulting / 
trajectory by considering all the occurrences of evolution by 
the first equation, followed by the second. 

The evolution given by the first equation in the time interval 
[0, T — k] can be simplified as follows. Set 



1 



Ih. 



t + tt 



Since be > 0, we get 



d — C,c c 

IM > ~, — : — IM-i 



(c + a 



(c + a 



fd-Cc\ k 
\Qc + a/ 

c (l-(d-(c) k /(Cc + a) k 



Cc + aV 1 - (d-Cc)/(Cc + a) 



Therefore, 



ir-K < 1 , 



'd-Cc 



1- 



(Cc+a)^- 



Xc + aJ C + lo (c + a\ 1 _ (^-Cc) 

(Cc+a) 



-c 



The claim now follows since It can be at most the above 
right hand side subject to the second, linear evolution for k 
time steps. ■ 

We can now prove Theorem III. 2. 

Proof of Theorem III. 2: Consider the recursion from 
Theorem III.l. Substituting z t := A(P t ), we obtain the 
following linear rational recurrence, 



zt< 



az 4 _! + b 
czt-i + d 



(11) 



where we used the definition of a, 6, c and d. 

Now, whenever x 6 X \ X$, z will evolve as per (11). 
Otherwise, z evolves as per 

zt < azt-i + b, 

which happens at most k times as per the assumption. There- 
fore, applying Lemma A. 2, the claim is established. ■ 
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Proof: Conditioning on the value of tt-\> we can write 
the following equality upon enumerating all possibilities of the 
sensors misdetecting. 

E[4|4-i] = (ait-i + &)((1 - pi) ... (1 - p m ) 

Pl(l ~p 2 ) ...(1- p m ) , , (1 -Pl)(l -Pl)---P m 



cilt-i + d\ c m £ t -i + d„ 

PlP2 ... (1 - p m ) (1 -pi) .. .Pm-lPm. 

C\lt-t-\ + <^12 Cm-l.mf-t-l + d m -l,m 



Pi... p n 



Cl,..., m 4-1 +dl,. 



Unconditioning on the random variable £t_i, and applying 
Jensen's inequality 2 to the concave function /(a;) = (ax + 
6) /(ex + d), we obtain the desired claim. ■ 

Proof: We begin with the equality 

E[£ t+1 \l t ] = (o£ + &)(1 -pi) . . . (1 - p m ) 

, Pl(l -P2) •••(!- Pm) , , (l-Pl)(l-P2)-.-Pm 



ci^t + di c m £ 4 + d n 

P1P2 ... (1 - gm) (1 -pi)...p m -ip m 

C\2h + di2 C m -i, m lt + d m -\ ,m 



Pi--- Pn 



Cl,..., m £t +di- 



We first collect all the terms that contain the term p\ in their 
numerator. For all these terms, observe that the denominator 
terms can be lower bounded by C\&t + di> since each of the c's 
are at least equal to c^ and likewise for the gTs. The numerator 
of those terms is each less than or equal to p\. Thus, we have 

E[£ t+1 \£ t ] < (a£ t + 6)(1 - m) . . . (1 - p m ) + Pl 

c\t t + d\ 

, (l-Pl)P2.--(l-Pm) , , (1-Pl)(l-P2)..-Pm 



c 2 £t + di c m / t + d m 

(1 - Pl)p 2 P3 • • ■ (1 ~Pm) , , (1 "Pl) ■• -Pm-lPn 



C23,£t + <^23 Cm-l.TO^t + «m-l,m 



Now, we repeat this procedure successively from the indices 2 
through m. Finally, upon unconditioning with respect to the 
random variable t t , and using Jensen's inequality for each 
of the concave linear fractional terms, we obtain the desired 
claim. ■ 



2 Jensen's inequality: For a random variable X with finite expectation, and 
a concave, real-valued function 4>{X), E[0(X)] < <j>(E[X]). 



